
#include "SameLength.h"

namespace common {

SameLength::SameLength(int id,Point* k,Point* l,Point* m,Point* n):common::Constraint(id,1,SAME_LENGTH){
	setK(k);
	setL(l);
	setM(m);
	setN(n);
}
ublasVector SameLength::getValue() throw(MyException){
	ublasVector out(1);
	out(0) = ((*getL()-*getK()).length()-(*getN()-*getM()).length());
	return out;
}
ublasMatrix SameLength::getJacobianOnPoint(Point* p) throw(MyException){

	ublasMatrix mout(1,2);

	Vector nmC((*getN()-*getM()).unit());
	Vector lkC((*getL()-*getK()).unit());
	ublasVector nm(2);
	nm(0) = nmC.getX(); nm(1) = nmC.getY();
	ublasVector lk(2);
	lk(0) = lkC.getX(); lk(1) = lkC.getY();

	if(k==p){
		row(mout,0)= -lk;
	}
	else if(l==p){
		row(mout,0)= lk;
	}
	else if(m==p){
		row(mout,0)= nm;
	}
	else if(n==p){
		row(mout,0) = -nm;
	}

	return mout;
}
SameLength::~SameLength(){

}

}
